package com.neuronrobotics.aim.gui;

import javax.swing.ImageIcon;
import javax.swing.JPanel;
import javax.swing.JFrame;
import java.awt.Dimension;
import java.awt.Toolkit;
import javax.swing.JTabbedPane;
import javax.swing.JButton;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;

import com.neuronrobotics.aim.activecannula.ActiveCannulaControlPanel;
import com.neuronrobotics.aim.robot.MRIController;
import com.neuronrobotics.nrconsole.plugin.PID.PIDControlGui;
import com.neuronrobotics.openigt.OpenIGTPanel;
import com.neuronrobotics.sdk.pid.IPIDEventListener;
import com.neuronrobotics.sdk.pid.PIDEvent;
import com.neuronrobotics.sdk.pid.PIDLimitEvent;
import com.neuronrobotics.sdk.ui.ConnectionDialog;
import net.miginfocom.swing.MigLayout;

public class MainGUI implements IPIDEventListener {

	public static void main(String[] args) {
		// @SuppressWarnings("unused")
		new MainGUI();
		// application.initialize();
	}

	private JTabbedPane jTabbedPane = null;
	
	private JPanel openIGTPanel = null;
	private ImageControlPanel imageControlPanel = null;
	private JPanel activeCannulaControlPanel = null;
	private JPanel jointControlPanel = null;
	private JPanel nrConsolePanel = null;
	
	
	private JFrame frame;
	private JButton connect = new JButton("Connect");
	private boolean connected = false;

	/** The device. */
	private MRIController device = new MRIController();

	/** The PID control. */
	private PIDControlGui pidControl;

	/** The current joint values. */
	private int[] currentJointValues = null;

	/**
	 * This is the default constructor.
	 */
	public MainGUI() {
		super();
		initialize();
	}

	/**
	 * Gets the main gui.
	 * 
	 * @return the main gui
	 */
	public MainGUI getMainGUI() {
		return this;
	}

	/**
	 * This method initializes this.
	 */
	private void initialize() {
		// JPanel->
		// JButton
		// JTabbedPane->
		// OpenIGTLink Panel
		// controlPanel
		// motionPanel

		JPanel content = new JPanel(new MigLayout("", "[]", "[][][]"));

		connect.addActionListener(new ActionListener() {
			public void actionPerformed(ActionEvent e) {
				if (!connected) {
					if (!ConnectionDialog.getBowlerDevice(getDevice())) {
						throw new RuntimeException("Failed to connect");
					}

					// Initialize current joint ticks values
					setCurrentJointTicksValues(getDevice().GetAllPIDPosition());
					// Initialize/setup desired joint ticks values
					imageControlPanel.setDesiredJointTicksValues(getDevice().GetAllPIDPosition());

					getDevice().addPIDEventListener(getMainGUI());

					pidControl = new PIDControlGui(getDevice());
					pidControl.setSize(new Dimension(1024, 768));
					nrConsolePanel.add(pidControl);

					// instantiate kinematics
					// new ProstateRobotKinematicModel();
					imageControlPanel.getRobotKinematics().setDevice(getDevice());
					
				} else {
					getDevice().disconnect();
				}
				connected = getDevice().isAvailable();
				if (connected) {
					connect.setText("Disconnect");
				} else {
					connect.setText("Connect");
				}
				getJTabbedPane().setVisible(connected);
				frame.setVisible(true);
			}
		});

		content.add(connect, "cell 0 0");
		content.add(getJTabbedPane(), "cell 0 1 1 2");
		getJTabbedPane().setVisible(false);
		// getJTabbedPane().setVisible(true);

		frame = new JFrame();
		frame.setContentPane(content);
		frame.setSize(new Dimension(1024, 768));
		frame.setPreferredSize(new Dimension(1024, 768));
		frame.setMaximumSize(new Dimension(2147, 2147));
		//
		Dimension screenSize = Toolkit.getDefaultToolkit().getScreenSize();
		// frame.setSize(new Dimension( 300, 300));
		Dimension windowSize = frame.getSize();

		int windowX = Math.max(0, (screenSize.width - windowSize.width) / 2);
		int windowY = Math.max(0, (screenSize.height - windowSize.height) / 2);
		frame.setLocation(windowX, windowY); // Don't use "f." inside
												// constructor.

		frame.setTitle("MRI guided  Prostate  Percutanous Intervention Robot Controller");
		// Kevin close when windows is closed
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.pack();
		frame.setVisible(true);
	}

	/**
	 * This method initializes jTabbedPane.
	 * 
	 * @return javax.swing.JTabbedPane
	 */
	private JTabbedPane getJTabbedPane() {
		if (jTabbedPane == null) {
			jTabbedPane = new JTabbedPane();
			jTabbedPane.setName("MRI Needle Robot Controller Tabs");
			jTabbedPane.setSize(new Dimension(1024, 768));
			jTabbedPane.setTabLayoutPolicy(JTabbedPane.SCROLL_TAB_LAYOUT);

			jTabbedPane.addTab("OpenIGTLink Panel",
					new ImageIcon(MainGUI.class.getResource("middle.gif")),
					getOpenIGTPanel(), null);
			
			jTabbedPane.addTab("Image Based Control Panel", new ImageIcon(
					MainGUI.class.getResource("middle.gif")),
					getImageControlPanel(), null);
			
			jTabbedPane.addTab("Joint Control Panel", new ImageIcon(
					MainGUI.class.getResource("middle.gif")), 
					getJointControlPanel(), null);
			
			jTabbedPane.addTab("Active Cannula Control Panel", new ImageIcon(
					MainGUI.class.getResource("middle.gif")), 
					getActiveCannulaControlPanel(), null);
			
			jTabbedPane.addTab("NR Console Panel", new ImageIcon(
					MainGUI.class.getResource("middle.gif")), getNRConsolePanel(),
					null);
		}
		return jTabbedPane;
	}
	
	/**
	 * This method initializes OpenIGTLinkPanel.
	 * 
	 * @return javax.swing.JPanel
	 */
	private JPanel getOpenIGTPanel() {
		if (openIGTPanel == null) {
			openIGTPanel = new OpenIGTPanel(this);
		}
		return openIGTPanel;
	}

	private JPanel getImageControlPanel() {
		if (imageControlPanel == null) {
			imageControlPanel = new ImageControlPanel(this);
		}
		return imageControlPanel;
	}

	private JPanel getActiveCannulaControlPanel() {
		if (activeCannulaControlPanel == null) {
			activeCannulaControlPanel = new ActiveCannulaControlPanel(this);
		}
		return activeCannulaControlPanel;
	}
	
	private JPanel getJointControlPanel() {
		if (jointControlPanel == null) {
			jointControlPanel = new JointJoggingPanel(this);
		}
		return jointControlPanel;
	}
	
	private JPanel getNRConsolePanel() {
		if (nrConsolePanel == null) {
			nrConsolePanel = new JPanel();
		}
		return nrConsolePanel;
	}
	

	/**
	 * Gets the device.
	 * @return the device
	 */
	public MRIController getDevice() {
		return device;
	}

	@Override
	public void onPIDEvent(PIDEvent e) {
//		System.out
//				.println("Got a PID event in Kinematics GUI #" + e.getGroup()
//						+ " value: " + e.getValue() + " timestamp: "
//						+ e.getTimeStamp());
//		// getCurrentJointTicksValues() is 8 vector
//		getCurrentJointTicksValues()[e.getGroup()] = e.getValue();
//		updateRobotStatus(getCurrentJointTicksValues());
	}

	@Override
	public void onPIDLimitEvent(PIDLimitEvent e) {
		// TODO Auto-generated method stub
	}

	@Override
	public void onPIDReset(int group, int currentValue) {
		// TODO Auto-generated method stub
	}

	
	
	/**
	 * ** set current joint tick values.
	 * 
	 * @param currentJointTicksValues
	 *            the new current joint ticks values integer array of current
	 *            joint ticks values
	 */
	public void setCurrentJointTicksValues(int[] currentJointTicksValues) {
		this.currentJointValues = currentJointTicksValues;
	}

//	/**
//	 * this thread checks to see if the values have been changed and updated the
//	 * motors if they are.
//	 * 
//	 * @author Hao Su
//	 * 
//	 */
//	private class Updater extends Thread {
//		public void run() {
//			while (getDevice().getConnection().isConnected()) {
//				// update rate 10Hz
//				ThreadUtil.wait(100);
//				if (isUpdateReady()) {
//					runDesiredJointUpdate(.1);
//				}
//			}
//		}
//	}

	/**
	 * ** read current joint tick values.
	 * 
	 * @return integer array of current joint ticks
	 */
	public int[] getCurrentJointTicksValues() {
		return currentJointValues;
	}

	/**
	 * round the array "longDigitsDouble" to 2 digits.
	 * 
	 * @param longDigitsDouble
	 *            the long digits double
	 * @return two digit double array
	 */
	public double[] doubleRound2TwoDigits(double[] longDigitsDouble) {
		for (int i = 0; i < longDigitsDouble.length; i++) {
			longDigitsDouble[i] = Math.round(longDigitsDouble[i] * 100) / 100.00;
		}
		return longDigitsDouble;
	}
	
	/**
	 * Double round2 three digits.
	 *
	 * @param longDigitsDouble the long digits double
	 * @return the double[]
	 */
	public double[] doubleRound2ThreeDigits(double[] longDigitsDouble) {
		for (int i = 0; i < longDigitsDouble.length; i++) {
			longDigitsDouble[i] = Math.round(longDigitsDouble[i] * 1000) / 1000.00;
		}
		return longDigitsDouble;
	}

	/**
	 * Sets the device.
	 * 
	 * @param device
	 *            the new device
	 */
	public void setDevice(MRIController device) {
		this.device = device;
	}

	/**
	 * On PID event, use the current joint ticks to calculate needle tip
	 * position and joint error.
	 * 
	 * @param currentJointTicks
	 *            the current joint ticks
	 * @return two digit double array currentJointTicks (should be 4x1 vector,
	 *         in DyIO is 8x1)
	 */

	public void updateRobotStatus(int[] currentJointTicks) {		
		// 1 step: convert joint encoder ticks to physical values (mm and degree)
		/** currentJointSIUnitValues is 6x1 vector*/
		double[] currentJointSIUnitValues = imageControlPanel.getRobotKinematics()
				.encoderTicks2JointSIUnits(currentJointTicks);
		// round the array "CurrentJointSIUnitValues" to 2 digits (6x1 vector)
		currentJointSIUnitValues = doubleRound2ThreeDigits(currentJointSIUnitValues);
		
		// 2 step: needle tip status from forward kinematics
		double[] cartesianSpaceVector = imageControlPanel.getRobotKinematics()
				.forwardKinematics(currentJointSIUnitValues);
		cartesianSpaceVector = doubleRound2TwoDigits(cartesianSpaceVector);
		
		// 3 step: read desired joint vector (6x1 vector) from ImageControlPanel
		// this only for status display in GUI
		double[] desiredJointSpaceVector = imageControlPanel.desiredJointSpaceVector;
		desiredJointSpaceVector = doubleRound2TwoDigits(desiredJointSpaceVector);
		
		// **************************************************************
		// 5 step: Update GUI display
		imageControlPanel.textFieldXAxisJointPosition.setText(Double
				.toString(currentJointSIUnitValues[0]));
		imageControlPanel.textFieldYAxisJointPosition.setText(Double
				.toString(currentJointSIUnitValues[1]));
		imageControlPanel.textFieldZAxisJointPosition.setText(Double
				.toString(currentJointSIUnitValues[2]));
		imageControlPanel.textFieldInsertionJointPosition.setText(Double
				.toString(currentJointSIUnitValues[3]));
		imageControlPanel.textFieldRotationJointPosition.setText(Double
				.toString(currentJointSIUnitValues[4]));
//		imageControlPanel.textFieldRetractionJointPosition.setText(Double
//				.toString(currentJointSIUnitValues[5]));

		imageControlPanel.textFieldXAxisJointError.setText(Double
				.toString(desiredJointSpaceVector[0]- currentJointSIUnitValues[0]));
		imageControlPanel.textFieldYAxisJointError.setText(Double
				.toString(desiredJointSpaceVector[1]- currentJointSIUnitValues[1]));
		imageControlPanel.textFieldInsertionJointError.setText(Double
				.toString(desiredJointSpaceVector[3]- currentJointSIUnitValues[3]));
		imageControlPanel.textFieldRotationJointError.setText(Double
				.toString(desiredJointSpaceVector[4]- currentJointSIUnitValues[4]));

		// update Cartesian space position
		// units are in mm and degree
		// wrt robot base coordinate

		// Display Cartesian space needle position
		imageControlPanel.tableActualNeedleRAS.setValueAt(cartesianSpaceVector[0],
				0, 3);
		imageControlPanel.tableActualNeedleRAS.setValueAt(cartesianSpaceVector[1],
				1, 3);
		imageControlPanel.tableActualNeedleRAS.setValueAt(cartesianSpaceVector[2],
				2, 3);
	}

}
